#include "main.h"
#include "pca9685.h"
#include "tmc2209.h"
#include "arm.h"
#include "tb6612.h"
#include "pstwo.h"
#include "mpu9250.h"
#include "inv_mpu9250_stm32F405port.h"

//分辨率 2^12 = 4096
#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600
#define SERVER_MAX_ANGLE 270
#define ARM_HEAD_LOCK_PERIOD 100

PWMServoDriver *pwm ;

TMC2209 *p_tmc2209;
int tmc2209_step;

TB6612 *p_tb6612;
int tb6612_period=0;

Joystick *p_ps2;

MPU9250 *p_mpu9250;
uint32_t last_time ;
// 欧拉角
float pitch, roll, yaw;
int count = 0;

I2C_HandleTypeDef *p_i2c1;
TIM_HandleTypeDef *p_tim13;
TIM_HandleTypeDef *p_tim14;

void run_init(I2C_HandleTypeDef *_p_i2c1,
		UART_HandleTypeDef *p_uart1, UART_HandleTypeDef *p_uart2
		,TIM_HandleTypeDef *_p_tim13,  TIM_HandleTypeDef *_p_tim14)
{
	p_i2c1 = _p_i2c1;
	p_tim13 = _p_tim13;
	p_tim14 = _p_tim14;

	pwm = new PWMServoDriver(p_i2c1, 0x80);
	p_tmc2209 =  new TMC2209(p_uart2);
	p_tb6612 =  new TB6612(p_tim13, &tb6612_period);

	p_ps2 = new Joystick();
	p_ps2->set_init();

	//p_mpu9250 = new MPU9250(p_i2c1);
	// p_mpu9250->Scan_I2C_Addresses();
	port_init(p_i2c1);



}
void run_once()
{
	last_time = HAL_GetTick();
	pwm->reset();
	HAL_Delay(1000);

	//舵机频率是50， 周期是20ms
	pwm->setPWMFreq(50);


}

void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
{
	if (htim->Instance  == TIM14 && tmc2209_step > 0)
	{
		//机械臂第1臂
		tmc2209_step = tmc2209_step - 1 ;

		__HAL_TIM_SET_COMPARE(p_tim14, TIM_CHANNEL_1, 10);

	}

	if (htim->Instance == TIM13)
	{
		//更换抓斗
		if (tb6612_period < ARM_HEAD_LOCK_PERIOD )
		{
			tb6612_period =+ 1 ;
			__HAL_TIM_SET_COMPARE(p_tim13, TIM_CHANNEL_1, 15);
		}
		else
		{
			p_tb6612->stop();
		}




	}

}
// 控制电机转动（使用STEP/DIR引脚）
void start_step_motor(int steps, int dir)
{
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, dir ? GPIO_PIN_SET : GPIO_PIN_RESET);
    tmc2209_step = steps;

    HAL_TIM_Base_Start_IT(p_tim14);
    HAL_TIM_PWM_Start_IT(p_tim14, TIM_CHANNEL_1);

    /*
    for(int i = 0; i < steps; i++)
    {
        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_SET);
        HAL_Delay(1);  // 可调整速度
        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET);
        HAL_Delay(1);
    }
    */
}

void run_loop()
{

	HAL_Delay(10);
	port_euler_angle();


	/*
	uint32_t current_time = HAL_GetTick();
	float dt = (current_time - last_time) / 1000.0f;
	        last_time = current_time;
	// 陀螺仪
	int16_t accel[3], gyro[3];
	p_mpu9250->read_accel(accel);  //加速度
	//printf("\r\n acc x=%d, y=%d, z=%d\r\n", accel[0], accel[1], accel[2]);


	DATA_printf(TX_DATA, T_X);  //转换X轴数据到数组
	Send_data('A', 'X');			 //发送X轴数
	DATA_printf(TX_DATA, T_Y);			 //转换Y轴数据到数组
	Send_data('A', 'Y');			 //发送Y轴数
	DATA_printf(TX_DATA, T_Z);			 //转换Z轴数据到数组
	Send_data('A', 'Z');			 //发送Z轴数
	*/

	/*
	//计算角度
	p_mpu9250->read_gyro(gyro);
	//printf("\r\n gyro x=%d, y=%d, z=%d\r\n", gyro[0], gyro[1], gyro[2]);
	// 更新四元数
	p_mpu9250->update_quaternion(accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2],
			dt);

	// 转换为欧拉角
	p_mpu9250->quaternion_euler(&pitch, &roll, &yaw);
	printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", pitch, roll, yaw);
	*/

	/*
	DATA_printf(TX_DATA, T_X);      //转换X轴数据到数组
	Send_data('G', 'X');			 //发送X轴数
	DATA_printf(TX_DATA, T_Y);			 //转换Y轴数据到数组
	Send_data('G', 'Y');			 //发送Y轴数
	DATA_printf(TX_DATA, T_Z);			 //转换Z轴数据到数组
	Send_data('G', 'Z');			 //发送Z轴数
	*/
	/*
	p_mpu9250->read_mag(data_acc);
	printf("\r\n magnet x=%d, y=%d, z=%d\r\n", data_acc[0], data_acc[1], data_acc[2]);
	*/

	HAL_Delay(10);
	/*
	READ_MPU9250_MAG();	      //磁场
	DATA_printf(TX_DATA, T_X);	      //转换X轴数据到数组
	Send_data('M', 'X');			 //发送X轴数
	DATA_printf(TX_DATA, T_Y);			 //转换Y轴数据到数组
	Send_data('M', 'Y');			 //发送Y轴数
	DATA_printf(TX_DATA, T_Z);			 //转换Z轴数据到数组
	Send_data('M', 'Z');			 //发送Z轴数
	*/
	/*
	// 收到PS2手柄控制
	uint8_t array_key[16]={0};
	HAL_Delay(50);
	p_ps2->data_key(array_key);
	for(uint8_t i =0 ; i < 16; i++)
	{
		if (array_key[i] != 0)                   //有按键按下
		{
			printf("  \r\n   %d  is  pressed  \r\n", array_key[i]);
		}
	}

	if(p_ps2->is_red_light())
	{
		printf(" %5d %5d %5d %5d\r\n", p_ps2->anolog_data(PSS_LX),
							p_ps2->anolog_data(PSS_LY), p_ps2->anolog_data(PSS_RX),
							p_ps2->anolog_data(PSS_RY));
	}


	HAL_Delay(50);
	*/


	/*
	// 假设收到uart命令

	// 正转500步
	StepMotor(500, 1);
	HAL_Delay(1000);

	// 反转500步
	StepMotor(500, 0);
	HAL_Delay(1000);
	if (CheckStallGuard(huart) == -1)
	{
		// 如果检测到堵转，则停止电机或其他处理逻辑。停止step

		//HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET); // 停止STEP信号
		HAL_TIM_PWM_Stop_IT(&htim14, TIM_CHANNEL_1);
		// 可以在此处添加其他处理逻辑，例如报警或改变方向等
	}


	HAL_Delay(1); // 根据实际情况调整延迟时间

	// 控制更换抓斗
	if (true)
	{
		// 锁

		p_tb6612->lock();
		HAL_Delay(3000);
		p_tb6612->unlock();
	}


	// 以下是控制舵机运行


	for (uint16_t angle = 0; angle < 3; angle++)
	{
		// (第几个舵机， 旋转角度on是多少， 最大角度)

		pwm->setPWM(0, 0, 180);
		pwm->setPWM(15, 0, SERVER_MAX_ANGLE);
		pwm->setPWM(4, 0, SERVER_MAX_ANGLE);
		pwm->setPWM(8, 0, SERVER_MAX_ANGLE);
		HAL_Delay(3000);

		pwm->setPWM(0, 90, 180);
		pwm->setPWM(15, 135, SERVER_MAX_ANGLE);
		pwm->setPWM(4, 135, SERVER_MAX_ANGLE);
		pwm->setPWM(8, 135, SERVER_MAX_ANGLE);
		HAL_Delay(3000);

		pwm->setPWM(0, 170, 180);
		pwm->setPWM(15, 260, SERVER_MAX_ANGLE);
		pwm->setPWM(4, 260, SERVER_MAX_ANGLE);
		pwm->setPWM(8, 260, SERVER_MAX_ANGLE);
		HAL_Delay(3000);


	}


	HAL_Delay(3000);
	*/


}
